
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_mission.h
  * @author     baiyang
  * @date       2022-5-12
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <mavproxy/mavproxy.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
// definitions
#define AP_MISSION_EEPROM_VERSION           0x65AE  // version number stored in first four bytes of eeprom.  increment this by one when eeprom format is changed
#define AP_MISSION_EEPROM_COMMAND_SIZE      15      // size in bytes of all mission commands

#ifndef AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
#define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 100     // allow up to 100 do-jump commands
#endif

#define AP_MISSION_JUMP_REPEAT_FOREVER      -1      // when do-jump command's repeat count is -1 this means endless repeat

#define AP_MISSION_CMD_ID_NONE              0       // mavlink cmd id of zero means invalid or missing command
#define AP_MISSION_CMD_INDEX_NONE           65535   // command index of 65535 means invalid or missing command
#define AP_MISSION_JUMP_TIMES_MAX           32767   // maximum number of times a jump can be executed.  Used when jump tracking fails (i.e. when too many jumps in mission)

#define AP_MISSION_FIRST_REAL_COMMAND       1       // command #0 reserved to hold home position

#define AP_MISSION_RESTART_DEFAULT          0       // resume the mission from the last command run by default

#define AP_MISSION_OPTIONS_DEFAULT          0       // Do not clear the mission when rebooting
#define AP_MISSION_MASK_MISSION_CLEAR       (1<<0)  // If set then Clear the mission on boot
#define AP_MISSION_MASK_DIST_TO_LAND_CALC   (1<<1)  // Allow distance to best landing calculation to be run on failsafe
#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2)  // Allow mission to continue after land

#define AP_MISSION_MAX_WP_HISTORY           7       // The maximum number of previous wp commands that will be stored from the active missions history
#define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2)

/*----------------------------------typedef-----------------------------------*/
#pragma pack(1)

// jump command structure
struct Jump_Command {
    uint16_t target;        // target command id
    int16_t num_times;      // num times to repeat.  -1 = repeat forever
};

// condition delay command structure
struct Conditional_Delay_Command {
    float seconds;          // period of delay in seconds
};

// condition delay command structure
struct Conditional_Distance_Command {
    float meters;           // distance from next waypoint in meters
};

// condition yaw command structure
struct Yaw_Command {
    float angle_deg;        // target angle in degrees (0=north, 90=east)
    float turn_rate_dps;    // turn rate in degrees / second (0=use default)
    int8_t direction;       // -1 = ccw, +1 = cw
    uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
};

// change speed command structure
struct Change_Speed_Command {
    uint8_t speed_type;     // 0=airspeed, 1=ground speed
    float target_ms;        // target speed in m/s, -1 means no change
    float throttle_pct;     // throttle as a percentage (i.e. 1 ~ 100), 0 means no change
};

// set relay command structure
struct Set_Relay_Command {
    uint8_t num;            // relay number from 1 to 4
    uint8_t state;          // on = 3.3V or 5V (depending upon board), off = 0V.  only used for do-set-relay, not for do-repeat-relay
};

// repeat relay command structure
struct Repeat_Relay_Command {
    uint8_t num;            // relay number from 1 to 4
    int16_t repeat_count;   // number of times to trigger the relay
    float cycle_time;       // cycle time in seconds (the time between peaks or the time the relay is on and off for each cycle?)
};

// set servo command structure
struct Set_Servo_Command {
    uint8_t channel;        // servo channel
    uint16_t pwm;           // pwm value for servo
};

// repeat servo command structure
struct Repeat_Servo_Command {
    uint8_t channel;        // servo channel
    uint16_t pwm;           // pwm value for servo
    int16_t repeat_count;   // number of times to move the servo (returns to trim in between)
    float cycle_time;       // cycle time in seconds (the time between peaks or the time the servo is at the specified pwm value for each cycle?)
};

// mount control command structure
struct Mount_Control {
    float pitch;            // pitch angle in degrees
    float roll;             // roll angle in degrees
    float yaw;              // yaw angle (relative to vehicle heading) in degrees
};

// digicam control command structure
struct Digicam_Configure {
    uint8_t shooting_mode;  // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6
    uint16_t shutter_speed;
    uint8_t aperture;       // F stop number * 10
    uint16_t ISO;           // 80, 100, 200, etc
    uint8_t exposure_type;
    uint8_t cmd_id;
    float engine_cutoff_time;   // seconds
};

// digicam control command structure
struct Digicam_Control {
    uint8_t session;        // 1 = on, 0 = off
    uint8_t zoom_pos;
    int8_t zoom_step;       // +1 = zoom in, -1 = zoom out
    uint8_t focus_lock;
    uint8_t shooting_cmd;
    uint8_t cmd_id;
};

// set cam trigger distance command structure
struct Cam_Trigg_Distance {
    float meters;           // distance
    uint8_t trigger;        // triggers one image capture immediately
};

// gripper command structure
struct Gripper_Command {
    uint8_t num;            // gripper number
    uint8_t action;         // action (0 = release, 1 = grab)
};

// AUX_FUNCTION command structure
struct AuxFunction {
    uint16_t function;  // from RC_Channel::AUX_FUNC
    uint8_t switchpos;  // from RC_Channel::AuxSwitchPos
};

// high altitude balloon altitude wait
struct Altitude_Wait {
    float altitude; // meters
    float descent_rate; // m/s
    uint8_t wiggle_time; // seconds
};

// nav guided command
struct Guided_Limits_Command {
    // max time is held in p1 field
    float alt_min;          // min alt below which the command will be aborted.  0 for no lower alt limit
    float alt_max;          // max alt above which the command will be aborted.  0 for no upper alt limit
    float horiz_max;        // max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
};

// do VTOL transition
struct Do_VTOL_Transition {
    uint8_t target_state;
};

// navigation delay command structure
struct Navigation_Delay_Command {
    float seconds; // period of delay in seconds
    int8_t hour_utc; // absolute time's hour (utc)
    int8_t min_utc; // absolute time's min (utc)
    int8_t sec_utc; // absolute time's sec (utc)
};

// DO_ENGINE_CONTROL support
struct Do_Engine_Control {
    bool start_control; // start or stop engine
    bool cold_start; // use cold start procedure
    uint16_t height_delay_cm; // height delay for start
};

// NAV_SET_YAW_SPEED support
struct Set_Yaw_Speed {
    float angle_deg;        // target angle in degrees (0=north, 90=east)
    float speed;            // speed in meters/second
    uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
};

// winch command structure
struct Winch_Command {
    uint8_t num;            // winch number
    uint8_t action;         // action (0 = relax, 1 = length control, 2 = rate control)
    float release_length;   // cable distance to unwind in meters, negative numbers to wind in cable
    float release_rate;     // release rate in meters/second
};

// Scripting command structure
struct scripting_Command {
    float p1;
    float p2;
    float p3;
};

// Scripting NAV command (with verify)
struct nav_script_time_Command {
    uint8_t command;
    uint8_t timeout_s;
    float arg1;
    float arg2;
};
#pragma pack()

union Mission_Content {
    // jump structure
    struct Jump_Command jump;

    // conditional delay
    struct Conditional_Delay_Command delay;

    // conditional distance
    struct Conditional_Distance_Command distance;

    // conditional yaw
    struct Yaw_Command yaw;

    // change speed
    struct Change_Speed_Command speed;

    // do-set-relay
    struct Set_Relay_Command relay;

    // do-repeat-relay
    struct Repeat_Relay_Command repeat_relay;

    // do-set-servo
    struct Set_Servo_Command servo;

    // do-repeate-servo
    struct Repeat_Servo_Command repeat_servo;

    // mount control
    struct Mount_Control mount_control;

    // camera configure
    struct Digicam_Configure digicam_configure;

    // camera control
    struct Digicam_Control digicam_control;

    // cam trigg distance
    struct Cam_Trigg_Distance cam_trigg_dist;

    // do-gripper
    struct Gripper_Command gripper;

    // arbitrary aux function
    struct AuxFunction auxfunction;

    // do-guided-limits
    struct Guided_Limits_Command guided_limits;

    // high altitude balloon altitude wait
    struct Altitude_Wait altitude_wait;

    // do vtol transition
    struct Do_VTOL_Transition do_vtol_transition;

    // DO_ENGINE_CONTROL
    struct Do_Engine_Control do_engine_control;

    // navigation delay
    struct Navigation_Delay_Command nav_delay;

    // NAV_SET_YAW_SPEED support
    struct Set_Yaw_Speed set_yaw_speed;

    // do-winch
    struct Winch_Command winch;

    // do scripting
    struct scripting_Command scripting;

    // nav scripting
    struct nav_script_time_Command nav_script_time;
    
    // location
    Location location;      // Waypoint location
};

// command structure
typedef struct {
    uint16_t index;             // this commands position in the command list
    uint16_t id;                // mavlink command id
    uint16_t p1;                // general purpose parameter 1
    union Mission_Content content;
} Mission_Command;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool mission_stored_in_location(uint16_t id);

MAV_MISSION_RESULT mission_SanityCheckParams(const mavlink_mission_item_int_t* packet);

// mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
//  return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_MavlinkIntToMissionCmd(const mavlink_mission_item_int_t* packet, Mission_Command* cmd);

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t *packet,
        mavlink_mission_item_int_t *mav_cmd);

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t *item_int,
        mavlink_mission_item_t *item);

// mavlink_cmd_long_to_mission_cmd - converts a mavlink cmd long to an AP_Mission::Mission_Command object which can be stored to eeprom
// return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_MavlinkCmdLongToMissionCmd(const mavlink_command_long_t* packet, Mission_Command* cmd);

// mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
//  return true on success, false on failure
bool mission_MissionCmdToMavlinkInt(const Mission_Command* cmd, mavlink_mission_item_int_t* packet);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



